/**********************************************************************************/
/* FILE NAME: Variable_OPS.c                                                      */
/*                                                                                */
/* DESCRIPTION:                                                                   */
/* This file contains functions for Reading and "Smoothing" Operating Variables   */
/* MAP_value, MAP_rate, TPS and TPS_rate.                                         */
/*                                                                                */
/*================================================================================*/
/* ORIGINAL AUTHOR:  Paul Schlein                                                 */
/* REV      AUTHOR          DATE          DESCRIPTION OF CHANGE                   */
/* ---     -----------     ----------    ---------------------                    */
/* 2.0     M. Eberhardt    20/Dec/11     added new table format                   */
/* 1.0     P. Schlein      12/Sep/11     Initial version                          */
/**********************************************************************************/

/*  General                                                                    */
#include "config.h"
#include "system.h"
#include "variables.h"
#include "Variable_OPS.h"
#include "etpu_util.h"
#include "Table_Lookup_JZ.h"
#include "eQADC_OPS.h"

/*  eTPU APIs                                                                  */
#include "etpu_toothgen.h"
#include "etpu_app_eng_pos.h"

/*  Other  */
#include "cpu.h"

#if 1

/* Global Declarations  */
#define MAX_AD_COUNTS  4096.
#define MAX_AD_VOLTAGE 5.

#define VBATT_VOLTAGE_DIVIDER (49.0/10.0)
#define V_Batt_AD    ADC_RsltQ0[1]          
#define CLT_VOLTAGE_DIVIDER 1.0
#define V_CLT_AD    ADC_RsltQ1[0]
#define IAT_VOLTAGE_DIVIDER 1.0
#define V_IAT_AD    ADC_RsltQ1[1]
#define TPS_VOLTAGE_DIVIDER 1.0
#define V_TPS_AD    ADC_RsltQ0[1]
#define MAP_1_VOLTAGE_DIVIDER 1.0
#define V_MAP_1_AD    ADC_RsltQ2 [0]
#define MAP_2_VOLTAGE_DIVIDER 1.0
#define V_MAP_2_AD    ADC_RsltQ1 [2]
#define MAP_3_VOLTAGE_DIVIDER 1.0
#define V_MAP_3_AD    ADC_RsltQ1 [3]
#define P1_VOLTAGE_DIVIDER 1.0
#define V_P1_AD    ADC_RsltQ1 [4]
#define P2_VOLTAGE_DIVIDER 1.0
#define V_P2_AD    ADC_RsltQ1 [5]
#define P3_VOLTAGE_DIVIDER 1.0
#define V_P3_AD    ADC_RsltQ1 [6]
#define P4_VOLTAGE_DIVIDER 1.0
#define V_P4_AD    ADC_RsltQ1 [7]
#define P5_VOLTAGE_DIVIDER 1.0
#define V_P5_AD    ADC_RsltQ1 [8]
#define P6_VOLTAGE_DIVIDER 1.0
#define V_P6_AD    ADC_RsltQ1 [9]
#define P7_VOLTAGE_DIVIDER 1.0
#define V_P7_AD    ADC_RsltQ1 [10]
#define P8_VOLTAGE_DIVIDER 1.0
#define V_P8_AD    ADC_RsltQ1 [11]
#define P9_VOLTAGE_DIVIDER 1.0
#define V_P9_AD    ADC_RsltQ1 [12]
#define P10_VOLTAGE_DIVIDER 1.0
#define V_P10_AD    ADC_RsltQ1 [13]
#define P11_VOLTAGE_DIVIDER 1.0
#define V_P11_AD    ADC_RsltQ1 [14]
#define P12_VOLTAGE_DIVIDER 1.0
#define V_P12_AD    ADC_RsltQ1 [15]
#define P13_VOLTAGE_DIVIDER 1.0
#define V_P13_AD    ADC_RsltQ1 [16]
#define P14_VOLTAGE_DIVIDER 1.0
#define V_P14_AD    ADC_RsltQ1 [17]
#define O2_1_UA_VOLTAGE_DIVIDER 1.0
#define V_O2_1_UA_AD    ADC_RsltQ1 [18]
#define O2_1_UR_VOLTAGE_DIVIDER 1.0
#define V_O2_1_UR_AD    ADC_RsltQ1 [19]
#define O2_2_UA_VOLTAGE_DIVIDER 1.0
#define V_O2_2_UA_AD    ADC_RsltQ1 [20]
#define O2_2_UR_VOLTAGE_DIVIDER 1.0
#define V_O2_2_UR_AD    ADC_RsltQ1 [21]
#define Knock_1_VOLTAGE_DIVIDER 1.0
#define V_Knock_1    ADC_RsltQ3 [0]
#define Knock_2_VOLTAGE_DIVIDER 1.0
#define V_Knock_2    ADC_RsltQ3 [2]

extern int32_t error_code;
// extern vuint16_t ADC_RsltQ0[4], ADC_RsltQ1[42], ADC_RsltQ2[2], ADC_RsltQ3[8];
extern uint32_t etpu_a_tcr1_freq;       //Implicit Defn.in eTPU_OPS.c
extern uint32_t etpu_b_tcr1_freq;       //Implicit Defn.in eTPU_OPS.c

/* End Global Declarations                                                        */

/**********************************************************************************/
/*                       C Code Functions                                         */
/**********************************************************************************/

/**********************************************************************************/
/* FUNCTION     : Get_Operational_Variables                                       */
/* PURPOSE      : This function Gets Operational Variables from the eQADCResult   */
/*                Queues and "smooths' them Using "Least Squares Estimator"       */
/* INPUT NOTES  : none                                                            */
/* RETURN NOTES : None                                                            */
/* WARNING      : "Least Squares Estimator" TBD-P. Schlein                        */
/**********************************************************************************/

void Get_Slow_Op_Vars(void)
{

/* Slow stuff...10hz or so*/
    V_CLT = (uint16_t) (V_CLT_AD * (uint32_t) ((uint32_t)((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * CLT_VOLTAGE_DIVIDER) << 20) >> 8);     // V_CLT is bin 12
    CLT = table_lookup_jz(V_CLT, 0, CLT_Table);

    V_IAT = (uint16_t) (V_IAT_AD * (uint32_t) ((uint32_t)((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * IAT_VOLTAGE_DIVIDER) << 20) >> 8);     // V_IAT is bin 12
    // remove fore testing
    //IAT = table_lookup_jz(V_IAT, 0, IAT_Table);

    V_MAP[1] = (uint16_t) (V_MAP_2_AD * (uint32_t) ((uint32_t)((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * MAP_2_VOLTAGE_DIVIDER) << 20) >> 8);     // V_MAP_2 is bin 12
    MAP[1] = table_lookup_jz(V_MAP[1], 0, MAP_2_Table);

    V_MAP[2] = (uint16_t) (V_MAP_3_AD * (uint32_t) ((uint32_t)((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * MAP_3_VOLTAGE_DIVIDER) << 20) >> 8);       // V_MAP_3 is bin 12
    MAP[2] = table_lookup_jz(V_MAP[2], 0, MAP_3_Table);

#if 0
    V_P1 = (uint16_t) (V_P1_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P1_VOLTAGE_DIVIDER) << 20) >> 8);    // V_P1 is bin 12
    V_P2 = (uint16_t) (V_P2_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P2_VOLTAGE_DIVIDER) << 20) >> 8);    // V_P2 is bin 12
    V_P3 = (uint16_t) (V_P3_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P3_VOLTAGE_DIVIDER) << 20) >> 8);    // V_P3 is bin 12
    V_P4 = (uint16_t) (V_P4_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P4_VOLTAGE_DIVIDER) << 20) >> 8);    // V_P4 is bin 12
    V_P5 = (uint16_t) (V_P5_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P5_VOLTAGE_DIVIDER) << 20) >> 8);    // V_P5 is bin 12
    V_P6 = (uint16_t) (V_P6_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P6_VOLTAGE_DIVIDER) << 20) >> 8);    // V_P6 is bin 12
    V_P7 = (uint16_t) (V_P7_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P7_VOLTAGE_DIVIDER) << 20) >> 8);    // V_P7 is bin 12
    V_P8 = (uint16_t) (V_P7_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P8_VOLTAGE_DIVIDER) << 20) >> 8);    // V_P8 is bin 12
    V_P9 = (uint16_t) (V_P9_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P9_VOLTAGE_DIVIDER) << 20) >> 8);    // V_P9 is bin 12
    V_P10 = (uint16_t) (V_P10_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P10_VOLTAGE_DIVIDER) << 20) >> 8); // V_P10 is bin 12
    V_P11 = (uint16_t) (V_P11_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P11_VOLTAGE_DIVIDER) << 20) >> 8); // V_P11 is bin 12
    V_P12 = (uint16_t) (V_P12_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P12_VOLTAGE_DIVIDER) << 20) >> 8); // V_P12 is bin 12
    V_P13 = (uint16_t) (V_P13_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P13_VOLTAGE_DIVIDER) << 20) >> 8); // V_P13 is bin 12
    V_P14 = (uint16_t) (V_P14_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_P14_VOLTAGE_DIVIDER) << 20) >> 8); // V_P14 is bin 12

    V_O2_1_UA = (uint16_t) (V_O2_1_UA_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_O2_1_UA_VOLTAGE_DIVIDER) << 20) >> 8);     // V_O2_1_UA is bin 12
    V_O2_1_UR = (uint16_t) (V_O2_1_UA_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_O2_1_UR_VOLTAGE_DIVIDER) << 20) >> 8);     // V_O2_1_UR is bin 12
    // AFR_1 = table_lookup_jz (V_O2_1,0, ARF_1_Table); //Need to know where this voltage comes from
    V_O2_2_UA = (uint16_t) (V_O2_2_UA_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_O2_2_UA_VOLTAGE_DIVIDER) << 20) >> 8);     // V_O2_2_UA is bin 12
    V_O2_2_UR = (uint16_t) (V_O2_2_UR_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * V_O2_2_UR_VOLTAGE_DIVIDER) << 20) >> 8);     // V_O2_2_UR is bin 12
    // AFR_2 = table_lookup_jz (V_O2_2,0, ARF_2_Table); //Need to know where this voltage comes from
#endif
}


// calc variables that need updating around every 10 msec

void Get_Fast_Op_Vars(void)
{

/* On fast for now, but should be medium speed ...100hz or so*/
    //Remove for testing
    //V_Batt = (uint16_t) ((V_Batt_AD * (uint32_t)((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * VBATT_VOLTAGE_DIVIDER) << 20) >> 10);        // V_Batt is bin 10
    RPM = fs_etpu_eng_pos_get_engine_speed(etpu_a_tcr1_freq);   //Read RPM

/* Fast speed stuff...1000hz or so*/
    V_TPS = (uint16_t) ((V_TPS_AD *   (uint32_t)((MAX_AD_VOLTAGE / MAX_AD_COUNTS) *   TPS_VOLTAGE_DIVIDER) << 20) >> 8);     // V_TPS is bin 12
    TPS = table_lookup_jz(V_TPS, 0, CLT_Table);
    
/* Angle based stuff */
    V_MAP[0] = (uint16_t) ((V_MAP_1_AD * (uint32_t)((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * MAP_1_VOLTAGE_DIVIDER) << 20) >> 8);       // V_MAP_1 is bin 12
    MAP[0] = table_lookup_jz(V_MAP[0], 0, MAP_1_Table);

#if 0

/*knock window based stuff*/
    V_knock_1 = (uint16_t) (Knock_1_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * Knock_1_VOLTAGE_DIVIDER) << 20) >> 8); // V_Knock_1 is bin 12
    V_knock_2 = (uint16_t) (Knock_2_AD * (uint32_t) (((MAX_AD_VOLTAGE / MAX_AD_COUNTS) * Knock_2_VOLTAGE_DIVIDER) << 20) >> 8); // V_Knock_2 is bin 12
#endif
}

#endif
